package org.sunspotworld.robowulf.irsensor;

import org.sunspotworld.robowulf.RobowulfServo;
import org.sunspotworld.util.Semaphore;
import org.sunspotworld.util.Units;

import com.sun.spot.util.Utils;
import com.sun.squawk.VM;

public class IRWheelController {
	private static final float WHEEL_DIAMETER_IN_METERS = 0.085F;
    private static final float AXLE_LENGTH_IN_METERS = 0.13F;

    private static final int NUM_WHEEL_STRIPES = 20;

    private static final float COUNTERS_PER_METER = NUM_WHEEL_STRIPES / (WHEEL_DIAMETER_IN_METERS * (float)Math.PI);
    private static final float COUNTERS_PER_DEGREE = 26F/360F;//1F/(float)Math.toDegrees(COUNTERS_PER_METER / (AXLE_LENGTH_IN_METERS / 2F)); //24F / 347F

    private static final int SLOW_END_ON_COUNTER = 3;
	private static final boolean DISPLAY_WHEEL_RATES = true;
	private static final int WHEEL_MONITOR_INTERVAL = 16;

	private IRWheelControlHelper[] controls;

    private IRSensor[] sensors;
	private RobowulfServo[] servos;
	private WheelState wheelState;

	private Semaphore sem;
	
	public IRWheelController(IRSensor leftSensor, IRSensor rightSensor, RobowulfServo leftServo, RobowulfServo rightServo) {
		sensors = new IRSensor[] { leftSensor, rightSensor };
		servos = new RobowulfServo[] { leftServo, rightServo };
		controls = new IRWheelControlHelper[] { new IRWheelControlHelper(0), new IRWheelControlHelper(1) };

		sem = new Semaphore(0);
		wheelState = new WheelState();
	}

	public void forward(float dist, int units) {
		move(dist, units, true);
	}

	public void backward(float dist, int units) {
		move(dist, units, false);
	}
	
	public void left(float degrees) {
		turn(degrees, true);
	}

	public void right(float degrees) {
		turn(degrees, false);
	}

	private void move(float dist, int units, boolean forward) {
		int counters = (int)Math.floor(Units.convert(dist, units, Units.METERS) * COUNTERS_PER_METER + 0.5F);
		activateServos(counters, forward, forward);
	}

	private void turn(float degrees, boolean left) {
		int counters = (int)Math.floor(degrees * COUNTERS_PER_DEGREE + 0.5F);
		activateServos(counters, left, !left);		
	}
	
	private void activateServos(int counters, boolean leftForward, boolean rightForward) {
		// Temporarily boost our thread priority so we get quick response to the wheels when the finish
		int oldP = Thread.currentThread().getPriority();
		Thread.currentThread().setPriority(Thread.MAX_PRIORITY);
		
		System.out.println("Resetting and adding listeners to IR sensors, starting servos");
		wheelState.reset(leftForward ? 1F : 0F, 	  rightForward ? 1F : 0F,
					     leftForward ? 0.6F : 0.4F, rightForward ? 0.6F : 0.4F);
		
		controls[0].start(counters);
		controls[1].start(counters);

		System.out.println("Waiting for sensor callbacks");
		sem.acquire();
		System.out.println("One wheel done");
		sem.acquire();
		System.out.println("Two wheels done");

		controls[0].stop();
		controls[1].stop();

		Utils.sleep(250L); // Both wheels will be braking right now, so wait 250ms to let the braking have an effect

		servos[0].floatServo();
		servos[1].floatServo();
		System.out.println("Stopped, done");

		try {
			System.out.println("Left counter: " + sensors[0].getConvertedValue());
			System.out.println("Right counter: " + sensors[1].getConvertedValue());
		} catch (Exception e) {}
		
		// Reset thread priority to its old value 
		Thread.currentThread().setPriority(oldP);
	}
	
	private final class IRWheelControlHelper implements IRSensorListener {
		private int wheel;
		private int counters;

		public IRWheelControlHelper(int wheel) {
			this.wheel = wheel;
		}
		
		public void start(int counters) {
			this.counters = counters;
			sensors[wheel].addIRSensorListener(this);
			sensors[wheel].resetCounter();

			synchronized (wheelState) {
				if (counters <= SLOW_END_ON_COUNTER) {
					servos[wheel].setPosition(wheelState.wheelSlowSpeed[wheel]);
				} else {
					servos[wheel].setPosition(wheelState.wheelSpeed[wheel]);
				}
			}
		}

		public void stop() {
			sensors[wheel].removeIRSensorListener(this);
		}

		public void counterIncremented(IRSensor src, int counterVal) {
			//System.out.print(counterVal);
			//System.out.print(',');
			//System.out.println(counters);
			if (counterVal == counters) {

				synchronized (wheelState) {
					wheelState.wheelStopped[wheel] = true;
					servos[wheel].brake();
				}
				System.out.println("WHEEL DONE, COUNTER = " + counterVal);
				sem.release();

			} else if (counterVal == counters - SLOW_END_ON_COUNTER) {
				
				System.out.println("WHEEL SLOW, COUNTER = " + counterVal);
				synchronized (wheelState) {
					 // If the other wheel hasn't been slowed yet, then we must slow both. We slow both wheels simultaneously so that the bot doesn't swerve
					if (!wheelState.wheelsSlowed) {
						wheelState.wheelsSlowed = true;
						
						// Slow the wheels that haven't stopped
						if (!wheelState.wheelStopped[0]) servos[0].setPosition(wheelState.wheelSlowSpeed[0]);
						if (!wheelState.wheelStopped[1]) servos[1].setPosition(wheelState.wheelSlowSpeed[1]);
					}
				}
				
			} else if (DISPLAY_WHEEL_RATES && counterVal < counters - SLOW_END_ON_COUNTER && counterVal % WHEEL_MONITOR_INTERVAL == 0) {

				long time = VM.getTimeMicros();
				synchronized (wheelState) {
					if (wheelState.monitorIntervalReached) {
						if (wheelState.monitorIntervalStartTime[wheel] != 0L) {
							long[] times = new long[2];
							
							times[wheel] = time - wheelState.monitorIntervalStartTime[wheel];
							times[1 - wheel] = wheelState.monitorIntervalReachedTime - wheelState.monitorIntervalStartTime[1 - wheel];

							float rateRatio = (float)times[0] / (float)times[1]; // r1 / r0

							System.out.println("Rate ratio (r1 / r0): " + rateRatio);
							
							//if (rateRatio < 0.98F || rateRatio > 1F/0.98F) {

								wheelState.speedFraction[0] *= rateRatio;
								normalizeWheelSpeedFractions();

								System.out.println("New wheel rates: 0: " + wheelState.speedFraction[0] + ", 1: " + wheelState.speedFraction[1]);

								setWheelSpeedFractions();
							//}
						}
						
						wheelState.monitorIntervalReached = false;
						wheelState.monitorIntervalStartTime[wheel] = time;
						wheelState.monitorIntervalStartTime[1 - wheel] = wheelState.monitorIntervalReachedTime;
					} else {
						wheelState.monitorIntervalReached = true;
						wheelState.monitorIntervalReachedTime = time;
					}
				}

			}
		}

		private void setWheelSpeedFractions() {
			servos[0].setPosition(0.5F + (wheelState.wheelSpeed[0] - 0.5F) * wheelState.speedFraction[0]);
			servos[1].setPosition(0.5F + (wheelState.wheelSpeed[1] - 0.5F) * wheelState.speedFraction[1]);
		}
		
		private void normalizeWheelSpeedFractions() {
			int greater;
			if (wheelState.speedFraction[0] > wheelState.speedFraction[1]) greater = 0;
			else														   greater = 1;
			
			if (wheelState.speedFraction[greater] != 1F) {
				wheelState.speedFraction[1 - greater] /= wheelState.speedFraction[greater];
				wheelState.speedFraction[greater] = 1F;
			}
		}
	}
	
	private static final class WheelState {
		public float[] wheelSpeed = new float[2];
		public float[] wheelSlowSpeed = new float[2];

		public boolean wheelsSlowed = false;
		public boolean[] wheelStopped = new boolean[2];

		public boolean monitorIntervalReached = false;
		public long monitorIntervalReachedTime = 0L;
		public long[] monitorIntervalStartTime = new long[2];
		public float[] speedFraction = new float[] { 1F, 1F };
		
		public void reset(float leftSpeed, float rightSpeed, float leftSlowSpeed, float rightSlowSpeed) {
			wheelSpeed[0] = leftSpeed;
			wheelSpeed[1] = rightSpeed;
			wheelSlowSpeed[0] = leftSlowSpeed;
			wheelSlowSpeed[1] = rightSlowSpeed;
			//speedFraction[0] = speedFraction[1] = 1F; 

			wheelsSlowed = false;
			wheelStopped[0] = wheelStopped[1] = false;
			
			monitorIntervalReached = false;
			monitorIntervalReachedTime = 0L;
			monitorIntervalStartTime[0] = monitorIntervalStartTime[1] = 0L;
		}
	}
}
